/*********************************************************************
*
* Software License Agreement (BSD License)
*
*  Copyright (c) 2010, Heriot-Watt University, UK.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Heriot-Watt University nor the names of
*     its contributors may be used to endorse or promote products
*     derived from this software without specific prior written
*     permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*
*  Author: Joel Cartwright
*
*********************************************************************/

#include <iostream>
#include <sstream>
using namespace std;

#include <ros/ros.h>
#include <osl_core/ModuleCore.h>
#include <auv_msgs/WorldWaypointReq.h>
#include <auv_msgs/WaypointSts.h>

class PilotWaypointIterator
{
private:
	bool waypointAchieved_;
	bool loopMode_;
	bool reading_;
	auv_msgs::WorldWaypointReq* currentMsg_;

	void printUsage(const std::string &progName)
	{
		cout << endl;
		cout << "usage:" << endl;
		cout << "  " << progName << " [--loop]" << endl;
		cout << "  Reads waypoints from stdin of the form:" << endl;
		cout << "     depth|altitude n e d|a pitch yaw" << endl;
		cout << "  depth or altitude requests either depth or altitude control of the z axis." << endl;
		cout << "  If 'n' is given for an axis value, control will be disabled for that axis." << endl;
		cout << "  North and east must either both be enabled or both disabled, as they" << endl;
		cout << "  affect x and y together." << endl;
		cout << "  A waypoint is attempted until its tolerance is reached. There is currently no timeout." << endl;
		cout << "  If the --loop argument is given, the provided waypoints will be repeated forever." << endl;
		cout << endl;
	}

	bool parseWaypointLine(std::istream & is, auv_msgs::WorldWaypointReq* msg)
	{
		cout << "Parsing new waypoint." << endl;
		int argPos = 0;

		std::string line;

		while (true) {
			getline(is, line, '\n');
			if (!is.good() && !is.eof()) {
				cerr << "Failed to get waypoint line." << endl;
				return false;
			}
			else if (is.eof()) {
				cout << "Reached end of waypoint list." << endl;
				reading_ = false;
				return false;
			}

			if (line == "" || line.at(0) == '#') {
				cout << "Ignoring empty/comment line." << endl;
			}
			else {
				break;
			}
		}

		stringstream ss(line);

		std::string argList[6];
		int totalArgs = 6;
		for (int i=0; i<totalArgs; ++i) {
			if (i < totalArgs - 1) {
			getline(ss, argList[i], ' ');
				if (!ss.good()) {
					cerr << "Failed to get item " << i << " from waypoint line." << endl;
					return false;
				}
			}
			else {
				getline(ss, argList[i]);
				if (!ss.good() && !ss.eof()) {
					cerr << "Failed to get item " << i << " from waypoint line." << endl;
					return false;
				}
			}
		}

		if (ss.good() && !ss.eof()) {
			std::string junk;
			getline(ss, junk);
			cout << "Ignoring junk at end of line: " << junk << endl;
		}

		if (argList[argPos] == "depth") {
			msg->altitude_mode = false;
		}
		else if (argList[argPos] == "altitude") {
			msg->altitude_mode = true;
		}
		else {
			cerr << "Invalid mode specifier: " << argList[argPos] << endl;
			return false;
		}
		++argPos;

		if (argList[argPos] == "n") msg->disable_axis.x = true;
		else msg->position.north = atof(argList[argPos].c_str());
		++argPos;

		if (argList[argPos] == "n") msg->disable_axis.y = true;
		else msg->position.east = atof(argList[argPos].c_str());
		++argPos;

		if ((msg->disable_axis.x || msg->disable_axis.y) && !(msg->disable_axis.x && msg->disable_axis.y)) {
			cerr << "North and east values must either both be valid, or both 'n'." << endl;
			return false;
		}

		if (argList[argPos] == "n") msg->disable_axis.z = true;
		else {
			if (msg->altitude_mode) {
				msg->altitude = atof(argList[argPos].c_str());
			}
			else {
				msg->position.depth = atof(argList[argPos].c_str());
			}
		}
		++argPos;

		if (argList[argPos] == "n") msg->disable_axis.pitch = true;
		else msg->orientation.pitch = osl_core::deg2rad(atof(argList[argPos].c_str()));
		++argPos;

		if (argList[argPos] == "n") msg->disable_axis.yaw = true;
		else msg->orientation.yaw = osl_core::deg2rad(atof(argList[argPos].c_str()));
		++argPos;

		cout << "Successfully parsed waypoint into msg:" << endl << *msg;

		return true;
	}

public:
	void waypointStsCallback(const auv_msgs::WaypointStsConstPtr & msg)
	{
		if (!waypointAchieved_ && currentMsg_ != NULL && msg->achieved) {
			if (currentMsg_->goal.id == msg->goal.id &&
					currentMsg_->goal.requester == msg->goal.requester) {
				cout << "Waypoint achieved: " << msg->goal.id << endl;
				waypointAchieved_ = true;
			}
		}
	}

	int main(int argc, char **argv)
	{
		waypointAchieved_ = true;
		loopMode_ = false;
		reading_ = true;
		currentMsg_ = NULL;

		string progName = basename(argv[0]);
		string nodeName = osl_core::ModuleCore::createROSNodeName(progName);

		unsigned currentIndex = 0;
		unsigned waypointID = 0;

		float positionTolerance = 0.2;
		float pitchTolerance = osl_core::deg2rad(20);
		float yawTolerance = osl_core::deg2rad(10);

		for (int i=0; i<argc; ++i)
		{
			if (strcmp(argv[i], "--help") == 0) {
				printUsage(progName);
				exit(0);
			}
			else if (strcmp(argv[i], "--loop") == 0) {
				cout << "Loop mode enabled. Waypoint list will be repeated forever." << endl;
				loopMode_ = true;
			}
		}

		ros::init(argc, argv, nodeName);
		ros::NodeHandle node;

		// Subscribed topics, with transport hints (earlier hints are preferred over later ones).
		ros::TransportHints hints = ros::TransportHints().udp().tcpNoDelay();
		ros::Subscriber waypointStsSub = node.subscribe<auv_msgs::WaypointSts>("pilot/waypoint_sts",
	 			1, &PilotWaypointIterator::waypointStsCallback, this, hints);

		ros::Publisher waypointPub = node.advertise<auv_msgs::WorldWaypointReq>("pilot/world_waypoint_req", 1);
		int rate = 5;

		std::vector<auv_msgs::WorldWaypointReq *> waypointList;

		ros::Rate loop_rate(rate);

		while (ros::ok())
		{
			if (waypointAchieved_) {
				waypointAchieved_ = false;
				currentMsg_ = NULL;
				++currentIndex;

				if (reading_) {
					currentMsg_ = new auv_msgs::WorldWaypointReq;
					currentMsg_->goal.requester = nodeName;
					currentMsg_->goal.priority = auv_msgs::GoalDescriptor::PRIORITY_NORMAL;
					currentMsg_->position_tolerance.x = positionTolerance;
					currentMsg_->position_tolerance.y = positionTolerance;
					currentMsg_->position_tolerance.z = positionTolerance;
					currentMsg_->orientation_tolerance.pitch = pitchTolerance;
					currentMsg_->orientation_tolerance.yaw = yawTolerance;

					if (!parseWaypointLine(cin, currentMsg_)) {
						if (!cin.eof()) {
							cerr << "Failed parsing waypoint. Stopping" << endl;
							break;
						}
						// If we get here, we have reached the end of the waypoint list.
						// Set reading to false and we will either repeat the first waypoint or stop below.
						reading_ = false;
					}
					else {
						waypointList.push_back(currentMsg_);
					}
				}

				if (!reading_)
				{
					if (loopMode_) {
						if (waypointList.size() == 0) {
							cerr << "No waypoints to replay - stopping." << endl;
							break;
						}
						if (currentIndex >= waypointList.size()) {
							currentIndex = 0;
						}
						currentMsg_ = waypointList[currentIndex];
						cout << "Replaying waypoint index " << currentIndex << endl << *currentMsg_;
					}
					else {
						cout << "End of waypoint list. Stopping." << endl;
						break;
					}
				}

				if (currentMsg_ != NULL) {
					++waypointID;
					currentMsg_->goal.id = waypointID;
				}
			}

			if (currentMsg_ != NULL) {
				currentMsg_->header.stamp = ros::Time::now();
				waypointPub.publish(*currentMsg_);
			}

			ros::spinOnce();
			loop_rate.sleep();
		}

		ros::shutdown();
		ros::waitForShutdown();

		return 0;
	}
};

// ---------------------------------------------------------------------------
int main(int argc, char **argv)
{
	PilotWaypointIterator waypointIter;
	return waypointIter.main(argc, argv);
}
